{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sys         ###mcl4header\n",
    "sys.path.append('../scripts/')\n",
    "from robot import *\n",
    "from scipy.stats import multivariate_normal  #追加"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "class Particle:\n",
    "    def __init__(self, init_pose):\n",
    "        self.pose = init_pose"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "class Mcl: ###Mcl4（draw以下省略）###\n",
    "    def __init__(self, init_pose, num, motion_noise_stds):   #引数追加\n",
    "        self.particles = [Particle(init_pose) for i in range(num)]\n",
    "        \n",
    "        v = motion_noise_stds #5-7行目追加\n",
    "        c = np.diag([v[\"nn\"]**2, v[\"no\"]**2, v[\"on\"]**2, v[\"oo\"]**2])\n",
    "        self.motion_noise_rate_pdf = multivariate_normal(cov=c)\n",
    "        \n",
    "    def motion_update(self, nu, omega, time): #追加\n",
    "        print(self.motion_noise_rate_pdf.cov)\n",
    "        \n",
    "    def draw(self, ax, elems): \n",
    "        xs = [p.pose[0] for p in self.particles]\n",
    "        ys = [p.pose[1] for p in self.particles]\n",
    "        vxs = [math.cos(p.pose[2]) for p in self.particles] \n",
    "        vys = [math.sin(p.pose[2]) for p in self.particles] \n",
    "        elems.append(ax.quiver(xs, ys, vxs, vys, color=\"blue\", alpha=0.5))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "class EstimationAgent(Agent):   ###EstimationAgent4###\n",
    "    def __init__(self, time_interval, nu, omega, estimator): #time_intervalを追加\n",
    "        super().__init__(nu, omega)\n",
    "        self.estimator = estimator\n",
    "        self.time_interval = time_interval #追加\n",
    "        \n",
    "    def draw(self, ax, elems):\n",
    "        self.estimator.draw(ax, elems)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[0.0001 0.     0.     0.    ]\n",
      " [0.     0.0004 0.     0.    ]\n",
      " [0.     0.     0.0009 0.    ]\n",
      " [0.     0.     0.     0.0016]]\n"
     ]
    }
   ],
   "source": [
    "initial_pose = np.array([0, 0, 0]).T   ###mcl_test4（下の行列出力も掲載）###\n",
    "estimator = Mcl(initial_pose, 100, motion_noise_stds={\"nn\":0.01, \"no\":0.02, \"on\":0.03, \"oo\":0.04})\n",
    "a = EstimationAgent(0.1, 0.2, 10.0/180*math.pi, estimator)\n",
    "estimator.motion_update(0.2, 10.0/180*math.pi, 0.1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[1 0]\n",
      " [0 2]]\n"
     ]
    }
   ],
   "source": [
    "print(np.diag([1,2])) ###diag###"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
